/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    QuadrpuedKernel.h
  * @author  Mentos Seetoo
  * @brief   Quadruped kernel includes following parts: Dynamics, State Machine,
  *          Controllers and Estimators. These are the basical module of a legged
  *          robot. In this file, we will migrate them into a single class to 
  *          make robot alive and add Application Interfaces for high level contol
  *           and users.			
  * @warning 
  *     - At least C++11 is required.														 
  *     - To view more details please visit: \n
  * 
  ******************************************************************************
  */
#ifndef QUADRUPED_KERNEL_H
#define QUADRUPED_KERNEL_H
/* Includes ------------------------------------------------------------------*/
#include "Motion_Scheduler/GaitScheduler.h"
#include "Motion_Scheduler/DesiredCoMState.h"
#include "Motion_Controller/BalanceController.h"
#include "Stste_Feedback/StateEstimator.h"
#include "Dynamics/dynamicModel.h"
#include "FSM/state_Locomotion.h"
#include "FSM/state_StandUp.h"
#include "FSM/state_Passive.h"
#include "FSM/state_Jump.h"
#include "FSM/state_Bounce.h"
/*Common Utilities*/
#include "matplotlibcpp.h"
#include "sys_thread.h"
#include "sys_log.h"

/* Types define --------------------------------------------------------------*/
namespace Quadruped
{
  /**
 * @brief Interfaces class
 */
  template <typename T>
  class QuadrupedKernel : public CThread
  {
  public:
    QuadrupedKernel(std::string thread_name, float period, ThreadType type);
    ~QuadrupedKernel();
    /**
     * @brief Dynamic model data.
     */
    Dynamic<T> _Dynamic;
    DMat<T> _A;
    DMat<T> _Ainv;
    DVec<T> _grav;
    DVec<T> _coriolis;
    LegData<T> _LegCommand[4];

    DesiredCoMState<T>   _CoMCommand;
    StateEstimator<T>    _StateEstimator;
  private:
    GaitScheduler<T>     _GaitScheduler;
    BalanceController<T> _BalanceController;
    /* Thread */
    void _UpdateModel();
    void init();
    void task();
    void clean_up() { ; }
  };
} // namespace Quadruped

#endif //QUADRUPED_KERNEL_H
       /************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
